clc; clear; close('all');
% Numero de Matricula: M11164
a = 1;
b = 1;
c = 1;
d = 6;
e = 4;
%% Datos del sistema
M = 0.10*(10+e);        % Kg
m = 0.02*(10+d);        % Kg
b = 0.01*(10+c);        % N/(m.s)
l = 0.03*(10+b);        % m
g = 9.81;               % m/s^2
X0 = [0 0 0.15 0]';     % \theta_0 = 0.15
tsim = 10;               % Segundos
t = (0:tsim/100:tsim)';

%% Modelo de estado del sistema linealizado
% Representacion fisica
A = [0      1           0               0
     0      -b/M        -m*g/M          0
     0      0           0               1
     0      b/(M*l)     (m+M)*g/(M*l)   0];
 
B = [0 1/M 0 -1/(M*l)]';
C = [1 0 0 0];
D = 0;
P = eig(A);     % Polos en cadena abierta
% Plot
pzplot(ss(A,B,C,D));
[y,x] = lsim(A,B,C,D,zeros(length(t),1),t,X0);
figure, plot(t,[x]);
title('Estados en cadena abierta. Entrada Nula. X0=[0 0 0.15 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap');
xlabel('Tiempo (s)'); grid on;

%% Realimentacion del estado
% Se verifica la controlabilidad del sistema
Q = [B   A*B   A^2*B   A^3*B];
rangoQ = rank(Q);
if rangoQ < length(B)
    errordlg('El sistema NO es Controlable');
    return
end
% Matriz de transformacion
Qinv = inv(Q);
e = Qinv(end,:);
Tcinv = [e ; e*A ; e*A^2 ; e*A^3];
Tc = inv(Tcinv);
% Representacion en la 2da forma canonica controlable
Ac = Tcinv*A*Tc;
Bc = Tcinv*B;
% Se asignan los polos
Pr = [-1 -1 -4 -6];
tmp = poly(Pr);
% Calculo de Kc
Kc = -Ac(end,:) - fliplr(tmp(2:end));
% Matriz realimentada en la 2da forma canonica controlable
Arc = Ac + Bc*Kc;
% Matriz realimentada en la representacion fisica
K = Kc*Tcinv;
Ar = A + B*K;
% Plot resultados
[y,x] = lsim(Ar,B,C,D,zeros(length(t),1),t,X0);
figure, plot(t,[x]);
title('Estados con Realimentacion. Entrada Nula. X0=[0 0 0.15 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap');
xlabel('Tiempo (s)'); grid on;

%% Mantener \theta por debajo de 0.01 radianes
tsim = 5;               % Segundos
t = (0:tsim/200:tsim)';
% Se re-asignan los polos
Pr = [-1 -5 -8 -10];
tmp = poly(Pr);
% Calculo de Kc
Kc = -Ac(end,:) - fliplr(tmp(2:end));
% Matriz realimentada en la 2da forma canonica controlable
Arc = Ac + Bc*Kc;
% Matriz realimentada en la representacion fisica
K = Kc*Tcinv;
Ar = A + B*K;
% Plot resultados
[y,x] = lsim(Ar,B,C,D,10*ones(length(t),1),t,X0);
figure, plot(t,[x]);
title('Estados con Realimentacion. Entrada = 10N. X0=[0 0 0.15 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap');
xlabel('Tiempo (s)'); grid on;
hold on;
% Lineas en +/-0.01
plot(t,ones(length(t))*0.01,'--k');
plot(t,ones(length(t))*-0.01,'--k');
lineCorte = 0:2.5/100:2.5;

%% Realimentacion del estado con observador
% Se verifica la observabilidad del sistema
P = [C;   C*A;   C*A^2;   C*A^3];
rangoP = rank(P);
if rangoP < length(B)
    errordlg('El sistema NO es Observable');
    return
end
% Matriz de transformacion
Pinv = inv(P);
e = Pinv(:,end);
To = [e  A*e  A^2*e  A^3*e];
Toinv = inv(To);
% Representacion en la 2da forma canonica observable
Ao=Toinv*A*To;
Bo=Toinv*B;
Co=C*To;
Do=D;
Ko = K * To;
X0e = Toinv*X0;
% Se asignan los polos del observador
Po = [-5 -10 -15 -20];
tmp = poly(Po);
% Calculo de H
H = Ao(:,end) + flipud(tmp(2:end)');
% Matriz F
F = Ao - H*Co;
% Sistema equivalente en la segunda forma observable
At = [Ao Bo*Ko;H*Co Ao-H*Co+Bo*Ko];
Bt = [Bo;Bo];
Ct = [Co Co];
Dt = Do;
X0o = Toinv*X0;
% Plot resultados
[y,xe]=lsim(At,Bt,Ct,Dt,zeros(length(t),1),t,[X0o ; X0e]);
To2 = [To zeros(size(To)) ; zeros(size(To)) To];
x=(To2*(xe'))';
figure, plot(t,[x]);
title('Realimentacion + Observador. Entrada Nula. X0=[0 0 0.15 0]^T. X0e=[0 0 0.15 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap','xe', 'xpe', '\thetae', '\thetape');
xlabel('Tiempo (s)'); grid on;
% Distintos estados iniciales
X0o = Toinv*[0 0 0 0]';
[y,xe]=lsim(At,Bt,Ct,Dt,zeros(length(t),1),t,[X0o ; X0e]);
To2 = [To zeros(size(To)) ; zeros(size(To)) To];
x=(To2*(xe'))';
figure, plot(t,[x]);
title('Realimentacion + Observador. Entrada Nula. X0=[0 0 0.15 0]^T. X0e=[0 0 0 0]^T','FontWeight','bold');
legend('x', 'xp', '\theta', '\thetap','xe', 'xpe', '\thetae', '\thetape');
xlabel('Tiempo (s)'); grid on;